import serial
import time
import logging
import binascii

logger = logging.getLogger(__name__)

class SerialProtocol:
    # 帧头和帧尾
    FRAME_HEADER = bytes([0xAA, 0xBB])
    FRAME_TAIL = bytes([0xCC, 0xDD])
    
    def __init__(self, port='/dev/ttyS1', baudrate=115200, pan_pin=3, tilt_pin=4, supported_pins=None):
        """
        初始化串口通信协议
        
        Args:
            port (str): 串口设备名
            baudrate (int): 波特率
            pan_pin (int): 水平舵机引脚
            tilt_pin (int): 垂直舵机引脚
            supported_pins (list): 支持的舵机引脚列表
        """
        self.port = port
        self.baudrate = baudrate
        self.pan_pin = pan_pin
        self.tilt_pin = tilt_pin
        self.supported_pins = supported_pins or [2, 3, 4, 6, 44, 45, 46, 7, 10]
        self.serial = None
        self.connected = False
        
    def connect(self):
        """连接到串口设备"""
        try:
            logger.info(f"尝试连接串口: {self.port}, 波特率: {self.baudrate}")
            self.serial = serial.Serial(self.port, self.baudrate, timeout=1)
            self.connected = True
            logger.info(f"已连接到串口设备，端口: {self.port}")
            
            # 等待串口稳定
            time.sleep(1)
            return True
            
        except Exception as e:
            logger.error(f"连接串口设备失败: {str(e)}")
            self.connected = False
            return False
    
    def disconnect(self):
        """断开与串口设备的连接"""
        if self.serial and self.serial.is_open:
            self.serial.close()
            self.connected = False
            logger.info("已断开与串口设备的连接")
    
    def calculate_checksum(self, data):
        """计算校验和"""
        return bytes([sum(data) & 0xFF])
    
    def angle_to_bytes(self, angle):
        """将角度转换为两个字节"""
        angle_int = int(angle * 10)  # 放大10倍以保留一位小数
        return bytes([angle_int >> 8, angle_int & 0xFF])
    
    def generate_protocol(self, servo_data):
        """
        生成舵机控制协议数据
        
        Args:
            servo_data (dict): 舵机数据，格式为 {引脚: 角度}
        
        Returns:
            bytes: 协议数据
        """
        # 计算有效舵机数量
        servo_count = len(servo_data)
        # 获取支持的引脚列表
        supported_pins = self.supported_pins
        # 协议固定为9个舵机位，检查配置是否超出
        if len(supported_pins) > 9:
             logger.warning(f"配置的 supported_pins 数量 ({len(supported_pins)}) 超过协议限制(9)，将只使用前9个")
             supported_pins = supported_pins[:9]
        if servo_count > len(supported_pins):
            # 如果要控制的舵机数量超过配置中支持的数量，可能需要报错或警告
            # 这里暂时按原逻辑，限制 servo_count 最大为9 （协议限制）
             logger.warning(f"要控制的舵机数量 ({servo_count}) 超过协议限制(9) 或配置支持的数量 ({len(supported_pins)})，最多处理9个")
             servo_count = min(servo_count, 9)
             # 或者可以抛出错误: raise ValueError("要控制的舵机数量超过配置中支持的数量")

        # 舵机数量 (使用计算后的 servo_count)
        data = bytearray([servo_count])
        
        # 添加舵机数据 (使用配置中的 supported_pins)
        # 协议假定有9个位置需要填充
        for i in range(9):
            if i < len(supported_pins):
                pin = supported_pins[i]
                if pin in servo_data:
                    # 添加舵机引脚
                    data.extend([pin])
                    # 添加舵机角度
                    data.extend(self.angle_to_bytes(servo_data[pin]))
                else:
                    # 未使用的舵机位置填充0 (对应 supported_pins 中的引脚)
                    data.extend([0, 0, 0])
            else:
                # 如果 supported_pins 不足9个，用0填充剩余协议位置
                data.extend([0, 0, 0])
        
        # 计算校验和
        checksum = self.calculate_checksum(self.FRAME_HEADER + data)
        
        # 组装完整协议数据
        protocol_data = self.FRAME_HEADER + data + checksum + self.FRAME_TAIL
        
        # 打印调试信息
        logger.debug(f"生成的协议数据: {protocol_data.hex()}")
        
        return protocol_data
    
    def send_data(self, data):
        """
        发送数据到串口
        
        Args:
            data (bytes): 要发送的数据
            
        Returns:
            bool: 是否发送成功
        """
        if not self.connected:
            logger.error("未连接到串口设备，无法发送数据")
            return False
            
        try:
            logger.info(f"发送的十六进制数据: {data.hex().upper()}")
            bytes_written = self.serial.write(data)
            logger.debug(f"已发送字节数: {bytes_written}")
            return True
        except Exception as e:
            logger.error(f"发送数据失败: {str(e)}")
            return False 